classdef person
    properties
        ass
        P
        X
        dt = 1;  %our sampling rate
        u = .005; % define acceleration magnitude
        HexAccel_noise_mag = .1; % noise of person
        Ex
        Ez
        A,B,C
        tkn_x = 1;  %measurement noise in the horizontal direction (x axis).
        tkn_y = 1;  %measurement noise in the horizontal direction (y axis).
        rec_size
        alpha = 0.1;
        found
    end
    methods
        function p = person(rec)
            p.Ez = [p.tkn_x 0; 0 p.tkn_y];
            dt = 1;
            p.A = [1 0 dt 0; 0 1 0 dt; 0 0 1 0; 0 0 0 1]; %state update matrice
            p.B = [(dt^2/2); (dt^2/2); dt; dt];
            p.C = [1 0 0 0; 0 1 0 0];  %this is our measurement function C, that we apply to the state estimate Q to get our expect next/new measurement
            p.Ex = [dt^4/4 0 dt^3/2 0; ...
                        0 dt^4/4 0 dt^3/2; ...
                        dt^3/2 0 dt^2 0; ...
                        0 dt^3/2 0 dt^2].*p.HexAccel_noise_mag^2; % Ex convert the process noise (stdv) into covariance matrix
            p.P = p.Ex;
            p.X = [double(rec(1));double(rec(2));0;0];
            p.rec_size = [rec(3);rec(4)];
            p.ass = rec(5);
        end
        
        function p = image_kalman_filter(p,Q_loc_meas)
            Q_loc_meas = double(Q_loc_meas);
            % Predict next state of the Hexbug with the last state and predicted motion.
            p.X = p.A * (p.X) + p.B * p.u;
            %predict next covariance
            p.P = p.A * p.P * p.A' + p.Ex;
            % predicted Ninja measurement covariance
            % Kalman Gain
            K = p.P*p.C'*inv(p.C*p.P*p.C'+p.Ez);
            % Update the state estimate.
            if ~isnan(Q_loc_meas(:))
                p.X = p.X + K * (Q_loc_meas(1:2)' - p.C * p.X);
            end
            % update covariance estimation.
            p.P =  (eye(4)-K*p.C)*p.P;
            
        end
        
        function p = KF_propagate(p)
            p.X = p.A * (p.X) + p.B * p.u;
            %predict next covariance
            p.P = p.A * p.P * p.A' + p.Ex;
            p.found = 0;
        end
        
        function p = KF_update(p,Q_loc_meas)
            Q_loc_meas = double(Q_loc_meas);
            K = p.P*p.C'*inv(p.C*p.P*p.C'+p.Ez);
            % Update the state estimate.
            if ~isnan(Q_loc_meas(:))
                p.X = p.X + K * (Q_loc_meas(1:2)' - p.C * p.X);
            end
            % update covariance estimation.
            p.P =  (eye(4)-K*p.C)*p.P;
        end
        
        function p = update_assurance(p,val)
            p.ass = p.ass* (1 - p.alpha) + val*p.alpha;
            p.found = 1;
        end
        
        function is = is_close_to_me(p,vec,rue)
            if  (p.X(1)-vec(1))^2 + (p.X(2)-vec(2))^2 < (rue)^2
                is = 1;
            else
                is = 0;
            end
        end
    end
end



